/*
* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty.  In no event will the authors be held liable for any damages
* arising from the use of this software.
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 3. This notice may not be removed or altered from any source distribution.
*/

#include "b2PairManager.h"
#include "b2BroadPhase.h"

#include <algorithm>

// Thomas Wang's hash.This aussume proxyId1 and proxyId2 are uint16.
inline static uint32 Hash(uint32 proxyId1, uint32 proxyId2)
{
	uint32 key = (proxyId2 << 16) | proxyId1;
	key = ~key + (key << 15);
	key = key ^ (key >> 12);
	key = key + (key << 2);
	key = key ^ (key >> 4);
	key = key * 2057;
	key = key ^ (key >> 16);
	return key;
}

inline static bool Equals(const b2Pair& pair, int32 proxyId1, int32 proxyId2)
{
	return (pair.proxyId1 == proxyId1) && (pair.proxyId2 == proxyId2);
}

inline static bool Equals(const b2BufferedPair& pair1, const b2BufferedPair& pair2)
{
	return (pair1.proxyId1 == pair2.proxyId1) && (pair2.proxyId2 == pair1.proxyId2);	
}

inline static bool operator <(const b2BufferedPair& pair1, const b2BufferedPair& pair2)
{
	if(pair1.proxyId1 < pair2.proxyId1)
	{
		return true;
	}
	else if(pair1.proxyId1 == pair2.proxyId1)
	{
		return pair1.proxyId2 < pair2.proxyId2;
	}

	return false;
}

b2PairManager::b2PairManager()
: m_freePair(0)
, m_pairCount(0)
, m_pairBufferCount(0)
{
	b2Assert(b2IsPowerOfTwo(b2_tableCapacity));
	b2Assert(b2_tableCapacity > b2_maxPairs);
	for(int32 i=0; i<b2_tableCapacity; ++i)
	{
		m_hashTable[i] = b2_nullPair;
	}

	for(int32 i=0; i<b2_maxPairs; ++i)
	{
		m_pairs[i].proxyId1 = b2_nullProxy;
		m_pairs[i].proxyId2 = b2_nullProxy;
		m_pairs[i].userData = NULL;
		m_pairs[i].status = 0;
		m_pairs[i].next = (uint16)(i+1);
	}
	m_pairs[b2_maxPairs-1].next = b2_nullPair;
}

void b2PairManager::Initialize(b2BroadPhase* broadPhase, b2PairCallback* callback)
{
	m_broadPhase = broadPhase;
	m_callback = callback;
}

b2Pair* b2PairManager::Find(int32 proxyId1, int32 proxyId2, uint32 hashValue)
{
	int32 index = m_hashTable[hashValue];
	while((index != b2_nullPair) && (Equals(m_pairs[index], proxyId1, proxyId2) == false))
	{
		index = m_pairs[index].next;
	}

	if(b2_nullPair == index)
	{
		return NULL;
	}

	b2Assert(index < b2_maxPairs);
	return  m_pairs + index;
}

b2Pair* b2PairManager::Find(int32 proxyId1, int32 proxyId2)
{
	if(proxyId1>proxyId2)
	{
		b2Swap(proxyId1, proxyId2);
	}

	int32 hash = Hash(proxyId1, proxyId2)  & b2_tableMask;
	return Find(proxyId1, proxyId2, hash);
}

// Return an existing pair or return a new created one.
b2Pair* b2PairManager::AddPair(int32 proxyId1, int32 proxyId2)
{
	if(proxyId1>proxyId2)
	{
		b2Swap(proxyId1, proxyId2);
	}

	int32 hash = Hash(proxyId1, proxyId2) & b2_tableMask;
	b2Pair* pair = Find(proxyId1, proxyId2, hash);
	if(NULL != pair)
	{
		return pair;
	}

	b2Assert((m_pairCount<b2_maxPairs) && (m_freePair != b2_nullPair));

	uint16 pairIndex = m_freePair;
	pair = m_pairs + pairIndex;
	m_freePair = pair->next;

	pair->proxyId1 = proxyId1;
	pair->proxyId2 = proxyId2;
	pair->status = 0;
	pair->userData = NULL;
	pair->next = m_hashTable[hash];

	m_hashTable[hash] = pairIndex;
	++m_pairCount;

	return pair;
}

// Remove a pair, the pair must exit.
void* b2PairManager::RemovePair(int32 proxyId1, int32 proxyId2)
{
	b2Assert(m_pairCount > 0);

	if(proxyId1 > proxyId2)
	{
		b2Swap(proxyId1, proxyId2);
	}

	int32 hash = Hash(proxyId1, proxyId2) & b2_tableMask;

	uint16* node = &m_hashTable[hash];	
	while(b2_nullPair != *node)
	{
		if(Equals(m_pairs[*node], proxyId1, proxyId2))
		{
			uint16 index = *node;
			*node = m_pairs[*node].next;
			
			b2Pair* pair = m_pairs + index;
			void* userData = pair->userData;

			//
			pair->next = m_freePair;
			pair->proxyId1 = b2_nullProxy;
			pair->proxyId2 = b2_nullProxy;
			pair->userData = NULL;
			pair->status = 0;

			m_freePair = index;
			--m_pairCount;
			return userData;
		}
		else
		{
			node = &m_pairs[*node].next;
		}
	}

	b2Assert(false);
	return NULL;
}

/*
As proxies are created and removed,many pairs are created and removed . enven worse, the same pair may be added and removed multiple times in a single time stem of 
physic engine. To reduce traffic in the pair manager, we try to avoid destroying pairs in the pair manager until the end of physic step,This done by buffering all the 
removeing pair request.Add pair request are processed immediately because we need the hash table entry for quick look up.

All user user call back are delayed until the buffer pairs are confirmed in commit.
This is very important because the user callback may be very expensive and client logic may be harmed if pairs are added and removed within the same step

Buffer a pair for addition.
We may add a pair that is not in tha pair manager or pair buffer.
we may add a pair that is already in pair manager and pair buffer.
If the added pair is not a new pair, then it must be int the pair buffer,(because remvoepair was called.)
*/
void b2PairManager::AddBufferedPair(int32 proxyId1, int32 proxyId2)
{
	b2Assert((b2_nullProxy!= proxyId1) && (b2_nullProxy!= proxyId2));
	b2Assert(m_pairBufferCount < b2_maxPairs);

	b2Pair* pair = AddPair(proxyId1, proxyId2);

	if(pair->IsBuffered() == false)
	{
		// This must be a newly added pair.
		b2Assert(pair->IsFinal() == false);

		// Add it to the pair buffer.
		pair->SetBuffered();
		m_pairBuffer[m_pairBufferCount].proxyId1 = pair->proxyId1;
		m_pairBuffer[m_pairBufferCount].proxyId2 = pair->proxyId2;
		++m_pairBufferCount;
		b2Assert(m_pairBufferCount < m_pairCount);
	}

	// Comfirm this pair for the subsequent call to commit.
	pair->ClearRemoved();

	if(b2BroadPhase::s_validate)
	{
		ValidateBuffer();
	}
}

// Buffer a pair for remove.
void b2PairManager::RemoveBufferedPair(int32 proxyId1, int32 proxyId2)
{
	b2Assert((b2_nullProxy!= proxyId1) && (b2_nullProxy!= proxyId2));
	b2Assert(m_pairBufferCount < b2_maxPairs);

	b2Pair* pair = Find(proxyId1, proxyId2);
	if(NULL == pair)
	{
		// The pair never exist, This is legal (due to collision filtering)
		return ;
	}

	// If the pair is not in the pair buffer.
	if(pair->IsBuffered() == false)
	{
		// This must be an old pair.
		b2Assert(pair->IsFinal() == true);
		pair->SetBuffered();

		m_pairBuffer[m_pairBufferCount].proxyId1 = pair->proxyId1;
		m_pairBuffer[m_pairBufferCount].proxyId2 = pair->proxyId2;
		++m_pairBufferCount;

		b2Assert(m_pairBufferCount <= m_pairCount);
	}

	pair->SetRemoved();

	if (b2BroadPhase::s_validate)
	{
		ValidateBuffer();
	}
}

void b2PairManager::Commit()
{
	int32 removeCount = 0;
	b2Proxy* proxies = m_broadPhase->m_proxyPool;

	for(int32 i=0; i<m_pairBufferCount; ++i)
	{
		b2Pair* pair = Find(m_pairBuffer[i].proxyId1, m_pairBuffer[i].proxyId2);
		b2Assert(pair->IsBuffered());
		pair->ClearBuffered();

		b2Assert((pair->proxyId1 < b2_maxProxies) && (pair->proxyId2 < b2_maxProxies));

		b2Proxy* proxy1 = proxies + pair->proxyId1;
		b2Proxy* proxy2 = proxies + pair->proxyId2;

		b2Assert(proxy1->IsValid());
		b2Assert(proxy2->IsValid());

		if(pair->IsRemoved())
		{
			// It is possible a pair was added and then removed befor commit. Therefore, 
			// we should be carefule not to tell the user the pair was removed when the user didn't recieve a match add.
			if(pair->IsFinal() == true)
			{
				m_callback->PairRemoved(proxy1->userData, proxy2->userData, pair->userData);
			}

			// Store the ids so we can acturally removed the pair below.
			m_pairBuffer[removeCount].proxyId1 = pair->proxyId1;
			m_pairBuffer[removeCount].proxyId2 = pair->proxyId2;
			++removeCount;
		}
		else
		{
			b2Assert(m_broadPhase->TestOverlap(proxy1, proxy2) == true);

			if(pair->IsFinal() == false)
			{
				pair->userData = m_callback->PairAdded(proxy1->userData, proxy2->userData);
				pair->SetFinal();
			}
		}

		for(int32 i=0; i<removeCount; ++i)
		{
			RemovePair(m_pairBuffer[i].proxyId1, m_pairBuffer[i].proxyId2);
		}
		m_pairBufferCount = 0;

		if (b2BroadPhase::s_validate)
		{
			ValidateTable();
		}
	}
}

void b2PairManager::ValidateBuffer()
{
#ifdef _DEBUG
	b2Assert(m_pairBufferCount < m_pairCount);

	std::sort(m_pairBuffer, m_pairBuffer + m_pairBufferCount);
	for(int32 i=0; i<m_pairBufferCount; ++i)
	{
		if(i>0)
		{
			b2Assert(Equals(m_pairBuffer[i], m_pairBuffer[i-1]) == false);
			b2Pair* pair = Find(m_pairBuffer[i].proxyId1, m_pairBuffer[i].proxyId2);
			b2Assert(pair->IsBuffered());

			b2Assert(pair->proxyId1 != pair->proxyId2);
			b2Assert(pair->proxyId1 < b2_maxProxies);
			b2Assert(pair->proxyId2 < b2_maxProxies);

			b2Proxy* proxy1 = m_broadPhase->m_proxyPool + pair->proxyId1;
			b2Proxy* proxy2 = m_broadPhase->m_proxyPool + pair->proxyId2;

			b2Assert(proxy1->IsValid() == true);
			b2Assert(proxy2->IsValid() == true);
		}
	}
#endif
}

void b2PairManager::ValidateTable()
{
#ifdef _DEBUG
	for (int32 i = 0; i < b2_tableCapacity; ++i)
	{
		uint16 index = m_hashTable[i];
		while (index != b2_nullPair)
		{
			b2Pair* pair = m_pairs + index;
			b2Assert(pair->IsBuffered() == false);
			b2Assert(pair->IsFinal() == true);
			b2Assert(pair->IsRemoved() == false);

			b2Assert(pair->proxyId1 != pair->proxyId2);
			b2Assert(pair->proxyId1 < b2_maxProxies);
			b2Assert(pair->proxyId2 < b2_maxProxies);

			b2Proxy* proxy1 = m_broadPhase->m_proxyPool + pair->proxyId1;
			b2Proxy* proxy2 = m_broadPhase->m_proxyPool + pair->proxyId2;

			b2Assert(proxy1->IsValid() == true);
			b2Assert(proxy2->IsValid() == true);

			b2Assert(m_broadPhase->TestOverlap(proxy1, proxy2) == true);

			index = pair->next;
		}
	}
#endif
}
